#include "SystemMatrix.hpp"

#include <iostream>
#include <vector>

#define _USE_MATH_DEFINES
#include <math.h>

#include "config.hpp"
#include "misc.hpp"
#include "TimedVoxels.hpp"
#include "Timogram.hpp"

using namespace std;

namespace Dynamic
{


SystemMatrix::SystemMatrix()
{
	array = new double[nlors * nvoxels]; Zero();
}

SystemMatrix::SystemMatrix(const SystemMatrix& m)
{
	array = new double[nlors * nvoxels];
	for (unsigned int i = 0; i < nvoxels * nlors; ++i) {
		array[i] = m.array[i];
	}
}

SystemMatrix::~SystemMatrix()
{
	delete [] array;
}

void SystemMatrix::operator=(const SystemMatrix& m)
{
	for (unsigned int i = 0; i < nvoxels * nlors; ++i) { array[i] = m.array[i]; }
}

double& SystemMatrix::at(unsigned int L, unsigned int V)
{
	if (L >= nlors || V >= nvoxels) {
		cerr << "Index error, L = " << L << ", V = " << V;
	}
	return array[nvoxels * L + V];
}

void SystemMatrix::Init(bool twoGauss /*= true*/)
{
	double detsize = R * 2 * M_PI / ndets;
	cout << "det size = " << detsize << "\n";
	double sigma = detsize / 2.4;

	Zero();

	double maxd = 0;
	for (int Y = 0; Y < vrows; Y++) {
		for (int X = 0; X < vrows; X++) {
			double x = volume * ((X + 0.5) / vrows - 0.5), y = volume * ((Y + 0.5) / vrows - 0.5);
			for (int iphi = 0; iphi < ndets; iphi++) {
				for (int idphi = gap + 1; idphi < ndets - iphi; idphi++) {
					int V = TimedVoxels::VoxelIndex(Y, X);
					int L = Timogram::LorIndex(iphi, idphi);
					double phi = 2.0 * M_PI * (iphi + 0.5) / ndets;
					double phi1 = 2.0 * M_PI * (iphi + idphi + 0.5) / ndets;
					double x1 = R * cos(phi), y1 = R * sin(phi);
					double x2 = R * cos(phi1), y2 = R * sin(phi1);
					double ny = x1 - x2, nx = y2 - y1;
					double nn = sqrt(nx * nx + ny * ny);
					nx /= nn; ny /= nn;
					double dist = nx * (x - x1) + ny * (y - y1);
					double v1x = x1 - x, v1y = y1 - y;
					double v1n = sqrt(v1x * v1x + v1y * v1y);
					v1x /= v1n; v1y /= v1n;
					double v2x = x2 - x, v2y = y2 - y;
					double v2n = sqrt(v2x * v2x + v2y * v2y);
					v2x /= v2n; v2y /= v2n;

					double alpha1 = acos(v1x * nx + v1y * ny), alpha2 = acos(v2x * nx + v2y * ny), alpha = acos(v1x * v2x + v1y * v2y);
					if (alpha1 + alpha2 + 0.001 > alpha) {  // in theory, they should be equal
						if (dist > maxd) { maxd = fabs(dist); }
						at(L, V) = Gauss2D(dist, sigma / 2) * 0.05 + (twoGauss ? Gauss2D(dist, sigma * 5) * 0.95 : 0);
					} else { at(L, V) = 0; }
				}
			}
		}
	}
	cerr << "\n max dist = " << maxd << '\n';
	for (unsigned int V = 0; V < nvoxels; ++V) {
		double colsum = 0;
		for (unsigned int L = 0; L < nlors; ++L) {
			colsum += at(L, V);
		}
		for (unsigned int L = 0; L < nlors; ++L) {
			at(L, V) /= colsum;
		}
	}
}

void SystemMatrix::Zero()
{
	for (unsigned int V = 0; V < nvoxels; ++V) {
		for (unsigned int L = 0; L < nlors; ++L) {
			at(L, V) = 0;
		}
	}
}

double SystemMatrix::sumByV(unsigned int voxelIndex)
{
	double sum = 0.0;
	for (unsigned int l = 0; l < nlors; l++) {
		sum += this->at(l, voxelIndex);
	}

	return sum;
}

void SystemMatrix::randomize(int nsamples)
{
	PoissonRandomize(array, nlors * nvoxels, nsamples);
}

void SystemMatrix::PositronRange()
{
	SystemMatrix result;
	/*      for ( unsigned int L = 0; L < nlors; ++L ) {
	for ( unsigned int V = 0; V < nvoxels; ++V ) {
	result.at(L, V) = at(L, V);
	if ( V > 1 ) result.at(L, V) += at(L, V-1);
	}
	}
	*/
	for (unsigned int L = 0; L < nlors; ++L) {
		for (int Y = 0; Y < vrows; Y++) {
			for (int X = 0; X < vrows; X++) {
				int V = TimedVoxels::VoxelIndex(Y, X);
				result.at(L, V) = 0;
				for (int Y1 = Y - 5; Y1 < Y + 5; Y1++) {
					for (int X1 = X - 5; X1 < X + 5; X1++) {
						if (X1 < 0 || X1 >= vrows || Y1 < 0 || Y1 >= vrows) {
							continue;
						}
						int V1 = TimedVoxels::VoxelIndex(Y1, X1);
						double dist = sqrt((double)(X - X1) * (X - X1) + (double)(Y - Y1) * (Y - Y1));
						result.at(L, V) += at(L, V1) * Gauss2D(dist, 1);
					}
				}
			}
		}
		//          cout << L << ", ";
	}

	for (unsigned int L = 0; L < nlors; ++L) {
		for (unsigned int V = 0; V < nvoxels; ++V) {
			at(L, V) = result.at(L, V);
		}
	}
}

void SystemMatrix::project(TimedVoxels& x, Timogram& yMeasComp)
{
	for (unsigned int L = 0; L < nlors; ++L) {
		std::vector<ListModeEvent>& lmes = yMeasComp.array[L];
		for (ListModeEvent & lme : lmes) {
			lme.lambda_Le = 0;
			for (unsigned int V = 0; V < nvoxels; ++V) {
				lme.lambda_Le += at(L, V) * x.getValueAtTime(V, lme.t_e);
			}
		}
	}
}

void SystemMatrix::backProject(TimedVoxels& x, Timogram& yMeasComp)
{
	float maxrat = 0;
	int maxL = 0;

	for (unsigned int V = 0; V < nvoxels; ++V) {
		for (unsigned int tId = 0; tId < x.timeSliceCount; tId++) {
			double denominator = 0.0; // SUM_L(A_LV * int_0_T(tau_i(t)*dt))
			double numerator = 0.0; // SUM_e(A_LV * tau_i(t_e) / lambda_L(t_e))
			double sum = 0.0;
			for (unsigned int L = 0; L < nlors; ++L) {
				double A_LV = at(L, V);
				if (A_LV < 0.001) {
					continue;
				}
				const std::vector<ListModeEvent>& lmes = yMeasComp.array[L];
				for (const ListModeEvent & lme : lmes) {
					if (lme.lambda_Le > 0.0) {
						numerator += A_LV * linearUniformTentBasisFunction(x.dt, tId, lme.t_e) / lme.lambda_Le;
					}
				}
			}

			for (unsigned int L = 0; L < nlors; ++L) {
				// QUESTION: Ez akkor nem csupa 1? mert a basis function integralja mindig 1 a full intervallumon
				// TODO: Ezt megcsinalni rendesen!
				denominator += at(L, V) * 1.0; // A_LV * int_0_T(tau_i(t)*dt)
			}

			if (denominator > 0.0) {
				//              double ratio = numerator / (denominator + TV_lambda * grad[V]);
				double ratio = numerator / denominator;

				x.at(V, tId) *= ratio;
				// Sides
// 				if ((V % vrows == 0) || (V % vrows == vrows - 1) ||
// 					(V / vrows == 0) || (V / vrows == vrows - 1)) {
// 						//cout << x.at(V, tId) << std::endl;
// 				} else {
// 					x.at(V, tId) *= ratio;
// 				}
			}
		}
	}
}

double SystemMatrix::Max()
{
	double maxvalue = 0;
	for (unsigned int L = 0; L < nlors; ++L) for (unsigned int V = 0; V < nvoxels; ++V) if (at(L, V) > maxvalue) { maxvalue = at(L, V); }
	return maxvalue;
}

void SystemMatrix::PrintImage(const char* ofname, double maxvalue, bool bar)
{
	int timeSliceCount = 1;
	double dt = 1;
	double measurementInterval = timeSliceCount * dt;
	TimedVoxels x(timeSliceCount, dt);
	x.Zero();
	x.at(10, 10, 0) = 1;

	Timogram y;
	y.InitWithOneEvent(measurementInterval / 2);
	this->project(x, y);
	y.PrintSinogram(ofname, maxvalue, bar);
}

}